using BepuPhysics.Collidables;
using BepuUtilities;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.CollisionDetection.CollisionTasks
{
    public struct CapsuleTriangleTester : IPairTester<CapsuleWide, TriangleWide, Convex2ContactManifoldWide>
    {
        public int BatchSize => 32;

        public static void TestEdge(in TriangleWide triangle, in Vector3Wide triangleCenter, in Vector3Wide triangleNormal,
            in Vector3Wide edgeStart, in Vector3Wide edgeOffset,
            in Vector3Wide capsuleCenter, in Vector3Wide capsuleAxis, in Vector<float> capsuleHalfLength,
            out Vector3Wide edgeDirection, out Vector<float> ta, out Vector<float> tb, out Vector<float> bMin, out Vector<float> bMax, out Vector<float> depth, out Vector3Wide normal)
        {
            // 借用胶囊测试的逻辑：
            // 计算两条直线段之间最近的点。一开始就不要夹紧。
            // 我们希望最小化距离=||(a+da*ta)-(b+db*tb)||。
            // 计算关于ta的导数并进行一些代数运算(考虑||da||=||db||==1)来求解ta的结果：
            // Ta=(da*(b-a)+(db*(a-b))*(da*db))/(1-((da*db)*(da*db)
            // (可能可以通过重新访问推导来允许非单位长度方向来避免平方根,但这是相当无关紧要的。)
            Vector3Wide.Length(edgeOffset, out var edgeLength);
            Vector3Wide.Scale(edgeOffset, Vector<float>.One / edgeLength, out edgeDirection);
            Vector3Wide.Subtract(edgeStart, capsuleCenter, out var offsetB);
            Vector3Wide.Dot(capsuleAxis, offsetB, out var daOffsetB);
            Vector3Wide.Dot(edgeDirection, offsetB, out var dbOffsetB);
            Vector3Wide.Dot(capsuleAxis, edgeDirection, out var dadb);
            // 请注意,当轴线平行时,电势除以零。任意钳位;接近零值会产生极值,而这些极值被钳位到合理的结果。
            ta = (daOffsetB - dbOffsetB * dadb) / Vector.Max(new Vector<float>(1e-15f), Vector<float>.One - dadb * dadb);
            // tb=ta*(da*db)-db*(b-a)
            tb = ta * dadb - dbOffsetB;

            // 我们不能简单地将ta和tb值钳制到线段。取而代之的是,将每条线段投影到另一条线段上,夹紧目标的间隔。
            // 该新的钳制投影间隔是该直线段上的有效解空间。我们可以用这个区间钳制t值,得到正确的有界解。
            // 预计间隔为：
            // B到A：(0或边长)*(da*db)+da*offsetB
            // A到B：+-AHalfLength*(da*db)-db*offsetB
            var ta0 = Vector.Max(-capsuleHalfLength, Vector.Min(capsuleHalfLength, daOffsetB));
            var ta1 = Vector.Min(capsuleHalfLength, Vector.Max(-capsuleHalfLength, daOffsetB + edgeLength * dadb));
            var aMin = Vector.Min(ta0, ta1);
            var aMax = Vector.Max(ta0, ta1);
            var aOntoBOffset = capsuleHalfLength * Vector.Abs(dadb);
            bMin = Vector.Max(Vector<float>.Zero, Vector.Min(edgeLength, -aOntoBOffset - dbOffsetB));
            bMax = Vector.Min(edgeLength, Vector.Max(Vector<float>.Zero, aOntoBOffset - dbOffsetB));
            ta = Vector.Min(Vector.Max(ta, aMin), aMax);
            tb = Vector.Min(Vector.Max(tb, bMin), bMax);

            Vector3Wide.Scale(capsuleAxis, ta, out var closestPointOnCapsule);
            Vector3Wide.Add(closestPointOnCapsule, capsuleCenter, out closestPointOnCapsule);
            Vector3Wide.Scale(edgeDirection, tb, out var closestPointOnEdge);
            Vector3Wide.Add(closestPointOnEdge, edgeStart, out closestPointOnEdge);

            Vector3Wide.Subtract(closestPointOnCapsule, closestPointOnEdge, out normal);
            Vector3Wide.LengthSquared(normal, out var normalLengthSquared);
            // 如果由于胶囊内部线段接触到边缘,法线长度为零,则使用边缘和轴的叉积。
            Vector3Wide.CrossWithoutOverlap(edgeOffset, capsuleAxis, out var fallbackNormal);
            Vector3Wide.LengthSquared(fallbackNormal, out var fallbackNormalLengthSquared);
            var useFallbackNormal = Vector.LessThan(normalLengthSquared, new Vector<float>(1e-15f));
            Vector3Wide.ConditionalSelect(useFallbackNormal, fallbackNormal, normal, out normal);
            normalLengthSquared = Vector.ConditionalSelect(useFallbackNormal, fallbackNormalLengthSquared, normalLengthSquared);
            // 不幸的是,如果边和轴是平行的,那么叉积也将是零,所以我们需要另一个后备。我们将使用边平面法线。
            // 除非三角形退化,否则长度不可能为零。
            Vector3Wide.CrossWithoutOverlap(triangleNormal, edgeOffset, out var secondFallbackNormal);
            Vector3Wide.LengthSquared(fallbackNormal, out var secondFallbackNormalLengthSquared);
            var useSecondFallbackNormal = Vector.LessThan(normalLengthSquared, new Vector<float>(1e-15f));
            Vector3Wide.ConditionalSelect(useSecondFallbackNormal, secondFallbackNormal, normal, out normal);
            normalLengthSquared = Vector.ConditionalSelect(useSecondFallbackNormal, secondFallbackNormalLengthSquared, normalLengthSquared);
            // 校准法线,使其从三角形指向太空舱。请注意,这可能与胶囊的"边缘"方向不同。
            Vector3Wide.Subtract(capsuleCenter, triangleCenter, out var ba);
            Vector3Wide.Dot(normal, ba, out var normalBA);
            var numerator = Vector.ConditionalSelect(Vector.LessThan(normalBA, Vector<float>.Zero), new Vector<float>(-1f), Vector<float>.One);
            Vector3Wide.Scale(normal, numerator / Vector.SquareRoot(normalLengthSquared), out normal);

            // 请注意,由于夹紧,最近点之间的法线不一定同时垂直于边缘和胶囊轴线,因此要计算深度
            // 我们需要包括这两种情况的程度。
            Vector3Wide.Dot(capsuleAxis, normal, out var nAxis);
            Vector3Wide.Dot(normal, capsuleCenter, out var nCapsuleCenter);
            // 法线校准为从三角形指向胶囊,因此极值点向下推。
            var extremeOnCapsule = nCapsuleCenter - Vector.Abs(nAxis) * capsuleHalfLength;
            Vector3Wide.Dot(triangle.A, normal, out var na);
            Vector3Wide.Dot(triangle.B, normal, out var nb);
            Vector3Wide.Dot(triangle.C, normal, out var nc);
            // 正常校准意味着最大三角值。
            var extremeOnTriangle = Vector.Max(na, Vector.Max(nb, nc));
            depth = extremeOnTriangle - extremeOnCapsule;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ClipAgainstEdgePlane(in Vector3Wide edgeStart, in Vector3Wide edgeOffset, in Vector3Wide faceNormal, in Vector3Wide capsuleCenter, in Vector3Wide capsuleAxis,
            out Vector<float> entry, out Vector<float> exit)
        {
            // t=-edgeToCapsule*(edgePlane Normal/||edgePlane Normal||)/(封装轴线*(edgePlane Normal/||edgePlane Normal||))
            Vector3Wide.CrossWithoutOverlap(faceNormal, edgeOffset, out var edgePlaneNormal);
            Vector3Wide.Subtract(capsuleCenter, edgeStart, out var edgeToCapsule);
            Vector3Wide.Dot(edgeToCapsule, edgePlaneNormal, out var distance);
            Vector3Wide.Dot(capsuleAxis, edgePlaneNormal, out var velocity);
            // 请注意,接近零的分母(平行轴)会产生一个有适当符号的大有限值。
            var velocityIsPositive = Vector.GreaterThan(velocity, Vector<float>.Zero);
            var t = Vector.ConditionalSelect(velocityIsPositive, -distance, distance) / Vector.Max(new Vector<float>(1e-15f), Vector.Abs(velocity));
            // 最后的间隔是max(entryAB,entryBC,entryCA)到min(exitAB,exitBC,exitCA)。
            // 如果光线方向与平面法线相反,则交点被视为"入口"。
            entry = Vector.ConditionalSelect(velocityIsPositive, new Vector<float>(-float.MaxValue), t);
            exit = Vector.ConditionalSelect(velocityIsPositive, t, new Vector<float>(float.MaxValue));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Test(
            ref CapsuleWide a, ref TriangleWide b, ref Vector<float> speculativeMargin,
            ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount,
            out Convex2ContactManifoldWide manifold)
        {
            manifold = default;
            // 在三角形的局部空间中工作,以限制变换要求。
            Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB);
            Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB);
            Vector3Wide.Negate(localOffsetB, out var localOffsetA);
            Vector3Wide.Add(b.A, b.B, out var localTriangleCenter);
            Vector3Wide.Add(b.C, localTriangleCenter, out localTriangleCenter);
            Vector3Wide.Scale(localTriangleCenter, new Vector<float>(1f / 3f), out localTriangleCenter);

            QuaternionWide.TransformUnitY(orientationA, out var worldCapsuleAxis);
            Matrix3x3Wide.TransformByTransposedWithoutOverlap(worldCapsuleAxis, rB, out var localCapsuleAxis);

            // 用于深度接触的分离轴有四个来源,其中胶囊轴与三角形相交：
            // 囊轴x AB
            // 胶囊轴x交流
            // 囊轴x BC
            // AC X AB
            // 然而,虽然这些对于线段三角形测试是足够的,但我们正在处理的是一个胶囊,它的非投机性接触需要最接近的点。
            // 这增加了更多潜在来源：
            // 胶囊端点VS面(AC x AB冗余)
            // 胶囊端点与边缘
            // 胶囊端点与顶点
            // 胶囊线对面(带有轴-边或端点对面的冗余)
            // 胶囊线与边(带有轴-边的冗余)
            // 胶囊线与顶点

            // 虽然显式执行这些附加测试可以确定最小分隔轴,但我们可以隐含地考虑它们。
            // 如果所有轴-边测试都计算边和胶囊轴有界线段之间的最近点,则会处理三角形顶点和胶囊端点。

            Vector3Wide.Subtract(b.C, b.A, out var ac);
            Vector3Wide.Subtract(b.B, b.A, out var ab);

            Vector3Wide.CrossWithoutOverlap(ac, ab, out var acxab);
            Vector3Wide.LengthSquared(acxab, out var faceNormalLengthSquared);
            Vector3Wide.Scale(acxab, Vector<float>.One / Vector.SquareRoot(faceNormalLengthSquared), out var faceNormal);

            Vector3Wide.Subtract(localOffsetA, localTriangleCenter, out var triangleCenterToCapsuleCenter);
            // 沿面法线的深度不受三角形范围的影响-三角形沿其自身法线没有范围。但是太空舱有。
            Vector3Wide.Dot(faceNormal, localCapsuleAxis, out var nDotAxis);
            Vector3Wide.Dot(faceNormal, triangleCenterToCapsuleCenter, out var capsuleOffsetAlongNormal);
            // 请注意,当胶囊的中心位于三角形的非碰撞一侧时,胶囊偏移AlongNormal可能为负数。
            // 这很好-要么边缘情况会产生更好的结果,要么根本不会生成接触。
            var faceDepth = a.HalfLength * Vector.Abs(nDotAxis) - capsuleOffsetAlongNormal;

            TestEdge(b, localTriangleCenter, faceNormal, b.A, ab, localOffsetA, localCapsuleAxis, a.HalfLength,
                out var edgeDirection, out var ta, out var tb, out var bMin, out var bMax, out var edgeDepth, out var edgeNormal);
            TestEdge(b, localTriangleCenter, faceNormal, b.A, ac, localOffsetA, localCapsuleAxis, a.HalfLength,
                out var edgeDirectionCandidate, out var taCandidate, out var tbCandidate, out var bMinCandidate, out var bMaxCandidate, out var edgeDepthCandidate, out var edgeNormalCandidate);
            var useAC = Vector.LessThan(edgeDepthCandidate, edgeDepth);
            Vector3Wide.ConditionalSelect(useAC, edgeDirectionCandidate, edgeDirection, out edgeDirection);
            Vector3Wide.ConditionalSelect(useAC, edgeNormalCandidate, edgeNormal, out edgeNormal);
            ta = Vector.ConditionalSelect(useAC, taCandidate, ta);
            tb = Vector.ConditionalSelect(useAC, tbCandidate, tb);
            bMin = Vector.ConditionalSelect(useAC, bMinCandidate, bMin);
            bMax = Vector.ConditionalSelect(useAC, bMaxCandidate, bMax);
            edgeDepth = Vector.Min(edgeDepthCandidate, edgeDepth);

            Vector3Wide.Subtract(b.C, b.B, out var bc);
            TestEdge(b, localTriangleCenter, faceNormal, b.B, bc, localOffsetA, localCapsuleAxis, a.HalfLength,
                out edgeDirectionCandidate, out taCandidate, out tbCandidate, out bMinCandidate, out bMaxCandidate, out edgeDepthCandidate, out edgeNormalCandidate);
            var useBC = Vector.LessThan(edgeDepthCandidate, edgeDepth);
            Vector3Wide.ConditionalSelect(useBC, b.B, b.A, out var edgeStart);
            Vector3Wide.ConditionalSelect(useBC, edgeDirectionCandidate, edgeDirection, out edgeDirection);
            Vector3Wide.ConditionalSelect(useBC, edgeNormalCandidate, edgeNormal, out edgeNormal);
            ta = Vector.ConditionalSelect(useBC, taCandidate, ta);
            tb = Vector.ConditionalSelect(useBC, tbCandidate, tb);
            bMin = Vector.ConditionalSelect(useBC, bMinCandidate, bMin);
            bMax = Vector.ConditionalSelect(useBC, bMaxCandidate, bMax);
            edgeDepth = Vector.Min(edgeDepthCandidate, edgeDepth);

            var depth = Vector.Min(edgeDepth, faceDepth);
            var useEdge = Vector.LessThan(edgeDepth, faceDepth);
            Vector3Wide.ConditionalSelect(useEdge, edgeNormal, faceNormal, out var localNormal);
            Vector3Wide.Dot(localNormal, faceNormal, out var localNormalDotFaceNormal);
            if (Vector.EqualsAll(Vector.BitwiseOr(
                    Vector.LessThanOrEqual(localNormalDotFaceNormal, Vector<float>.Zero),
                    Vector.LessThan(depth + a.Radius, -speculativeMargin)), new Vector<int>(-1)))
            {
                // 所有的接触法线都在三角形的背面,或者距离对于边距来说太大了,所以我们可以立即退出。
                manifold.Contact0Exists = Vector<int>.Zero;
                manifold.Contact1Exists = Vector<int>.Zero;
                return;
            }
            Vector3Wide b0 = default, b1 = default;
            Vector<int> contactCount;
            if (Vector.EqualsAny(useEdge, new Vector<int>(-1)))
            {
                // 至少有一条路径使用边,因此请继续创建所有边联系相关信息。
                // 再次向胶囊借用-胶囊：
                // 在两个胶囊轴线共面的情况下,我们接受整个间隔作为接触源。
                // 当轴线偏离共面时,可接受的间隔迅速缩小到零长度,以ta和Tb为中心。
                // 我们根据胶囊轴线与由盒边和接触法线定义的平面之间的夹角来评定共面度：
                // sin(角度)=点(da,(Db X Normal)/||db x Normal||)
                // 最后,请注意,我们处理的是极小的角度,并且对于小角度sin(Angle)~=角度,
                // 而且淡入淡出的行为是完全任意的,所以我们可以直接使用平方角,而不用担心。
                // 角度^2~=点(da,(Db X Normal))^2/||db x Normal|^2
                // 请注意,如果||db x Normal||为零,则任何da都应被接受为共面,因为没有限制。条件选择不连续的部分。
                Vector3Wide.CrossWithoutOverlap(edgeDirection, edgeNormal, out var planeNormal);
                Vector3Wide.LengthSquared(planeNormal, out var planeNormalLengthSquared);
                Vector3Wide.Dot(localCapsuleAxis, planeNormal, out var numeratorUnsquared);
                var squaredAngle = Vector.ConditionalSelect(Vector.LessThan(planeNormalLengthSquared, new Vector<float>(1e-10f)), Vector<float>.Zero, numeratorUnsquared * numeratorUnsquared / planeNormalLengthSquared);

                // 将平方角度转换为LERP参数。对于从0到LowerThreshold的平方角度,我们应该使用完整的间隔(1)。从lowerThreshold到upperThreshold,将LERP设置为0。
                const float lowerThresholdAngle = 0.01f;
                const float upperThresholdAngle = 0.05f;
                const float lowerThreshold = lowerThresholdAngle * lowerThresholdAngle;
                const float upperThreshold = upperThresholdAngle * upperThresholdAngle;
                var intervalWeight = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, (new Vector<float>(upperThreshold) - squaredAngle) * new Vector<float>(1f / (upperThreshold - lowerThreshold))));
                // 请注意,我们使用的是Tb,即edge参数,而不是胶囊值。由于边界平滑,与三角形相关的接触必须位于三角形上。
                var weightedTb = tb - tb * intervalWeight;
                bMin = intervalWeight * bMin + weightedTb;
                bMax = intervalWeight * bMax + weightedTb;


                Vector3Wide.Scale(edgeDirection, bMin, out b0);
                Vector3Wide.Add(b0, edgeStart, out b0);
                Vector3Wide.Scale(edgeDirection, bMax, out b1);
                Vector3Wide.Add(b1, edgeStart, out b1);
                contactCount = Vector.ConditionalSelect(useEdge, Vector.ConditionalSelect(Vector.GreaterThan(bMax, bMin), new Vector<int>(2), Vector<int>.One), Vector<int>.Zero);
            }
            else
            {
                // 不使用边;所有触点必须是面触点。
                contactCount = Vector<int>.Zero;
            }


            // 任何边贡献现在都存储在b0、b1、localNormal和Depth字段中。
            // 1)如果面接触获胜(没有边提供任何接触),则通过将胶囊轴向三角形边缘平面剪裁来生成两个接触。
            // (使用裁剪是为了让#2共享结果,如果胶囊轴线与脸部表面平行,那么"应该"的边就不会赢。)
            // 2)如果边缘接触获胜且仅生成一个接触,则尝试通过以下方式生成一个附加面接触
            // 如果胶囊端点位于碰撞侧并且在三角形的边界内,则将胶囊端点投影到三角形上。
            // 边界检查可以共享对面接触所做的裁剪。
            // 3)如果边缘接触已生成两个接触,则不需要额外的接触。

            if (Vector.LessThanOrEqualAny(contactCount, Vector<int>.One))
            {
                ClipAgainstEdgePlane(b.A, ab, faceNormal, localOffsetA, localCapsuleAxis, out var abEntry, out var abExit);
                ClipAgainstEdgePlane(b.B, bc, faceNormal, localOffsetA, localCapsuleAxis, out var bcEntry, out var bcExit);
                // 缠绕很重要。不列颠哥伦比亚省AB市
                Vector3Wide.Negate(ac, out var ca);
                ClipAgainstEdgePlane(b.A, ca, faceNormal, localOffsetA, localCapsuleAxis, out var caEntry, out var caExit);
                var triangleIntervalMin = Vector.Max(abEntry, Vector.Max(bcEntry, caEntry));
                var triangleIntervalMax = Vector.Min(abExit, Vector.Min(bcExit, caExit));

                var negativeHalfLength = -a.HalfLength;
                var overlapIntervalMin = Vector.Max(triangleIntervalMin, negativeHalfLength);
                var overlapIntervalMax = Vector.Min(triangleIntervalMax, a.HalfLength);
                // 为了产生良好的面部接触,我们将从两边夹紧,但这意味着一个接触案例不会有单方面的夹紧间隔。
                // 那就提前进行测试吧。
                var intervalIsValidForSecondContact = Vector.GreaterThanOrEqual(overlapIntervalMax, overlapIntervalMin);
                overlapIntervalMin = Vector.Min(overlapIntervalMin, a.HalfLength);
                overlapIntervalMax = Vector.Max(overlapIntervalMax, negativeHalfLength);
                Vector3Wide.Scale(localCapsuleAxis, overlapIntervalMin, out var clippedOnA0);
                Vector3Wide.Add(clippedOnA0, localOffsetA, out clippedOnA0);
                Vector3Wide.Dot(clippedOnA0, faceNormal, out var nA0);
                Vector3Wide.Dot(localTriangleCenter, faceNormal, out var trianglePlaneOffset);
                var distanceAlongNormalA0 = nA0 - trianglePlaneOffset;
                Vector3Wide.Scale(faceNormal, distanceAlongNormalA0, out var toRemoveA0);
                Vector3Wide.Subtract(clippedOnA0, toRemoveA0, out var faceCandidate0);

                Vector3Wide.Scale(localCapsuleAxis, overlapIntervalMax, out var clippedOnA1);
                Vector3Wide.Add(clippedOnA1, localOffsetA, out clippedOnA1);
                Vector3Wide.Dot(clippedOnA1, faceNormal, out var nA1);
                var distanceAlongNormalA1 = nA1 - trianglePlaneOffset;
                Vector3Wide.Scale(faceNormal, distanceAlongNormalA1, out var toRemoveA1);
                Vector3Wide.Subtract(clippedOnA1, toRemoveA1, out var faceCandidate1);

                // 如果没有联系人,则自动接受这两个候选人。
                // 请注意,即使间隔长度为负值,我们也会接受它-如果没有边接触,则表示面法线是最佳选项。
                // 如果面法线是最佳选项,则必须有面接触。在裁剪方面的任何分歧都只是一个数字误差问题。
                // 作为单纯的数值误差的副作用,投影点与三角形之间的距离应该趋向于接近ε。
                // 一个例外情况是：如果胶囊的中心在三角形的背面,则不应创建任何触点。我们不想让接触者把物体"拉"过去-
                // 这会在复杂的网格中造成频繁的恶劣情况。
                var noEdgeContacts = Vector.Equals(contactCount, Vector<int>.Zero);
                var allowFaceContacts = Vector.GreaterThanOrEqual(capsuleOffsetAlongNormal, Vector<float>.Zero);
                var useFaceContacts = Vector.BitwiseAnd(noEdgeContacts, allowFaceContacts);
                Vector3Wide.ConditionalSelect(useFaceContacts, faceCandidate0, b0, out b0);
                Vector3Wide.ConditionalSelect(useFaceContacts, faceCandidate1, b1, out b1);
                contactCount = Vector.ConditionalSelect(useFaceContacts, new Vector<int>(2), contactCount);

                // 如果有一个边缘接触,则应该只接受两个面接触中的一个。
                // 请注意,两个修剪的间隔端点中的一个很可能最终会非常接近边缘接触,因此选择那个端点将是一种浪费。
                // 根据哪个修剪间隔端点距离胶囊轴上的边接触更远来选择端点。
                var useFaceContact1ForSecondContact = Vector.GreaterThan(Vector.Abs(overlapIntervalMax - ta), Vector.Abs(overlapIntervalMin - ta));
                Vector3Wide.ConditionalSelect(useFaceContact1ForSecondContact, faceCandidate1, faceCandidate0, out var secondContactCandidate);
                var secondContactDistanceAlongNormal = Vector.ConditionalSelect(useFaceContact1ForSecondContact, distanceAlongNormalA1, distanceAlongNormalA0);
                // 要将此联系人实际用作第二个联系人,必须满足以下条件：
                // 1)间隔有效(max>min)。
                // 2)联系人数==1。
                // 3)候选人在三角形上方。
                var useCandidateForSecondContact = Vector.BitwiseAnd(intervalIsValidForSecondContact,
                    Vector.BitwiseAnd(Vector.Equals(contactCount, Vector<int>.One), Vector.GreaterThan(secondContactDistanceAlongNormal, Vector<float>.Zero)));
                Vector3Wide.ConditionalSelect(useCandidateForSecondContact, secondContactCandidate, b1, out b1);
                contactCount = Vector.ConditionalSelect(useCandidateForSecondContact, new Vector<int>(2), contactCount);
            }

            // 测量两个触点的深度,看看它们沿着法线离胶囊有多远。
            // 我们将通过创建一个位于胶囊中心的平面来实现这一点,该平面的法线指向接触法线。
            // 胶囊轴平面法线=(N x胶囊轴)x胶囊轴
            // 深度是从接触位置开始,沿接触法线针对该平面测试光线得出的。
            Vector3Wide.CrossWithoutOverlap(localNormal, localCapsuleAxis, out var nxa);
            Vector3Wide.CrossWithoutOverlap(nxa, localCapsuleAxis, out var capsuleAxisPlaneNormal);
            // 如果生成的平面法线接近于零,则胶囊轴和面法线大致平行。
            // 在这种情况下,我们可以简单地使用翻转的面法线。
            Vector3Wide.LengthSquared(capsuleAxisPlaneNormal, out var capsuleAxisPlaneNormalLengthSquared);
            Vector3Wide.Negate(localNormal, out var negatedNormal);
            Vector3Wide.ConditionalSelect(Vector.LessThan(capsuleAxisPlaneNormalLengthSquared, new Vector<float>(1e-15f)), negatedNormal, capsuleAxisPlaneNormal, out capsuleAxisPlaneNormal);
            // 假设我们有一个特殊情况,它创建了一个不垂直于胶囊轴的平面法线,我们需要为胶囊平面选择一个中心点
            // 这给了我们想要的结果-两个触点都有相同的深度,与更靠近三角形的胶囊端点相匹配。
            Vector3Wide.Dot(capsuleAxisPlaneNormal, localCapsuleAxis, out var capsuleAxisPlaneDot);
            var planeAnchorOnCapsuleT = Vector.ConditionalSelect(Vector.LessThan(capsuleAxisPlaneDot, Vector<float>.Zero), -a.HalfLength, a.HalfLength);
            Vector3Wide.Scale(localCapsuleAxis, planeAnchorOnCapsuleT, out var capsulePlaneAnchor);
            Vector3Wide.Add(capsulePlaneAnchor, localOffsetA, out capsulePlaneAnchor);

            Vector3Wide.Subtract(capsulePlaneAnchor, b0, out var offset0);
            Vector3Wide.Dot(capsuleAxisPlaneNormal, offset0, out var planeDistance0);
            Vector3Wide.Subtract(capsulePlaneAnchor, b1, out var offset1);
            Vector3Wide.Dot(capsuleAxisPlaneNormal, offset1, out var planeDistance1);
            Vector3Wide.Dot(localNormal, capsuleAxisPlaneNormal, out var velocity);
            var inverseVelocity = Vector<float>.One / velocity;
            var separation0 = planeDistance0 * inverseVelocity;
            var separation1 = planeDistance1 * inverseVelocity;
            manifold.Depth0 = a.Radius - separation0;
            manifold.Depth1 = a.Radius - separation1;
            // 虽然零速度在数学上不应该出现,但在数值上是可能的。
            // 如果发生这种情况,我们会认为这是一种极其暂时的状态,只需使用黑客攻击即可。
            // (应该不会出现零速度,因为局部法线指向胶囊轴线,因此胶囊AxisPlane和法线垂直的唯一方法
            // (法线轴和胶囊轴是平行的,这在前面是显式保护的。)
            Debug.Assert(Vector.EqualsAll(Vector.BitwiseOr(
                Vector.GreaterThan(Vector.Abs(velocity), new Vector<float>(1e-15f)),
                Vector.OnesComplement(Vector.Equals(velocity, velocity))), new Vector<int>(-1)), "Velocity should be nonzero except in numerical corner cases.");
            var useFallbackDepth = Vector.LessThan(Vector.Abs(velocity), new Vector<float>(1e-15f));
            manifold.Depth0 = Vector.ConditionalSelect(useFallbackDepth, Vector<float>.Zero, manifold.Depth0);
            manifold.Depth1 = Vector.ConditionalSelect(useFallbackDepth, Vector<float>.Zero, manifold.Depth1);

            // 对于功能ID,请注意我们有几个不同的潜在联系人来源。虽然我们可以检查并强制每个潜在的源输出ID,
            // 有一个有用的统一因素：接触发生在胶囊轴线上的位置。使用此功能,无论接触是从面还是从边缘情况生成都无关紧要,
            // 因此,正确重用前一帧的累积脉冲的机会更高。
            // 使用深度,将触点向胶囊推出,然后计算它们沿胶囊轴线的投影位置。换句话说,就是：
            // TA0=((b0+N*深度0)-胶囊中心)*胶囊轴
            // TA1=((b1+N*深度1)-胶囊中心)*胶囊轴
            // 如果TA0>TA1,则FeatureId0=1,FeatureId1=0,否则,FeatureId0=0,FeatureId1=1。
            // 这会做一些多余的计算,但重做这项工作比跟踪所有潜在联系人来源所需的一切要简单得多。
            Vector3Wide.Scale(localNormal, manifold.Depth0, out var toAOffset0);
            Vector3Wide.Add(b0, toAOffset0, out var a0);
            Vector3Wide.Subtract(a0, localOffsetA, out var capsuleToA0);
            Vector3Wide.Dot(capsuleToA0, localCapsuleAxis, out var ta0);
            Vector3Wide.Scale(localNormal, manifold.Depth1, out var toAOffset1);
            Vector3Wide.Add(b1, toAOffset1, out var a1);
            Vector3Wide.Subtract(a1, localOffsetA, out var capsuleToA1);
            Vector3Wide.Dot(capsuleToA1, localCapsuleAxis, out var ta1);
            var flipFeatureIds = Vector.LessThan(ta1, ta0);
            manifold.FeatureId0 = Vector.ConditionalSelect(flipFeatureIds, Vector<int>.One, Vector<int>.Zero);
            manifold.FeatureId1 = Vector.ConditionalSelect(flipFeatureIds, Vector<int>.Zero, Vector<int>.One);

            var faceFlag = Vector.ConditionalSelect(
                Vector.GreaterThanOrEqual(localNormalDotFaceNormal, new Vector<float>(MeshReduction.MinimumDotForFaceCollision)),
                new Vector<int>(MeshReduction.FaceCollisionFlag), Vector<int>.Zero);
            manifold.FeatureId0 += faceFlag;

            // 将接触位置变换为世界空间旋转,以相对于胶囊(对象A)的偏移进行测量。
            Matrix3x3Wide.TransformWithoutOverlap(b0, rB, out var contact0RelativeToB);
            Vector3Wide.Add(contact0RelativeToB, offsetB, out manifold.OffsetA0);
            Matrix3x3Wide.TransformWithoutOverlap(b1, rB, out var contact1RelativeToB);
            Vector3Wide.Add(contact1RelativeToB, offsetB, out manifold.OffsetA1);
            Matrix3x3Wide.TransformWithoutOverlap(localNormal, rB, out manifold.Normal);

            // 如果我们发现法线远离三角形法线,那么它击中了错误的一侧,应该忽略。(请注意,我们早些时候已经提前完成了这项工作。)
            contactCount = Vector.ConditionalSelect(Vector.GreaterThanOrEqual(localNormalDotFaceNormal, Vector<float>.Zero), contactCount, Vector<int>.Zero);
            var depthThreshold = -speculativeMargin;
            manifold.Contact0Exists = Vector.BitwiseAnd(Vector.GreaterThan(manifold.Depth0, depthThreshold), Vector.GreaterThan(contactCount, Vector<int>.Zero));
            manifold.Contact1Exists = Vector.BitwiseAnd(Vector.GreaterThan(manifold.Depth1, depthThreshold), Vector.GreaterThan(contactCount, Vector<int>.One));

        }


        public void Test(ref CapsuleWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out Convex2ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }

        public void Test(ref CapsuleWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex2ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }
    }
}
